#include "msp430g2553.h" // Include the necessary header for our target MCU
#define IRA1 !(P2IN & BIT4)
#define IRB1 !(P2IN & BIT5) 

#define MOTORA1_ON P2OUT |= BIT0
#define MOTORA2_ON P2OUT |= BIT1 
#define MOTORB1_ON P2OUT |= BIT2
#define MOTORB2_ON P2OUT |= BIT3 

#define MOTORA1_OFF P2OUT &= ~BIT0
#define MOTORA2_OFF P2OUT &= ~BIT1 
#define MOTORB1_OFF P2OUT &= ~BIT2
#define MOTORB2_OFF P2OUT &= ~BIT3 

#define RF_A !(P1IN & BIT2)
#define RF_B !(P1IN & BIT3)
#define RF_C !(P1IN & BIT4)
#define RF_D !(P1IN & BIT5)

void delay_ms(unsigned int ms ){ 		// Not really a milliseconds delay!
      unsigned int i;
      for (i = 0;i<= ms; i++){ 		// The operator is <=, I've yet to fix this site bug.
        __delay_cycles(500);
      }
}
 
void Forward1()
{
	MOTORA1_OFF;
	MOTORA2_OFF;	
}

void Forward2()
{
	MOTORB1_OFF;
	MOTORB2_OFF;	
}

void Reverse1()
{
	MOTORA1_ON;
	MOTORA2_ON;
}

void Reverse2()
{
	MOTORB1_ON;
	MOTORB2_ON;
}

void Stop1()
{
	MOTORA1_ON;
	MOTORA2_OFF;
}

void Stop2()
{
	MOTORB1_ON;
	MOTORB2_OFF;
}

void main(void) {
 
	WDTCTL = WDTPW + WDTHOLD;		// Hold the WDT (WatchDog Timer)
	P2DIR |= BIT0|BIT1|BIT2|BIT3;       		// Setup the appropriate output bits	
 		
			Stop1();
			Stop2();
while(1) {

		 /*if( ((RF_C)) ){				// Otherwise...
			while( (!(IRA1))||(!(IRB1)) ){
				if(!(IRA1)){Forward1();}
				if(!(IRB1)){Forward2();}
			}
			Stop1();
			Stop2();
			
			Reverse1();
			Reverse2();
 			delay_ms(5);
			while( (!(IRA1))||(!(IRB1)) ){
				if(!(IRA1)){Reverse1();}
				if(!(IRB1)){Reverse2();}
			}
			Stop1();
			Stop2();
		}
		else*/ if((RF_B) ){				// Otherwise...
			while( !(IRB1) ){Forward2();}
			Stop2();
			Reverse2();
 			delay_ms(5);
			while( !(IRB1) ){Reverse2();}
			Stop2();
		}
		else if ( (RF_A) ){		// If not (port one input AND bit3 (sw1))
			while( !(IRA1) ){Forward1();}
			Stop1();
			Reverse1();
 			delay_ms(5);
			while( !(IRA1) ){Reverse1();}
			Stop1();
		}
 delay_ms(500);
 
}

}